#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <iostream>
#include <string>

int new_msg_flag;
geometry_msgs::PoseStamped pos;
geometry_msgs::TwistStamped vel;
geometry_msgs::PoseStamped pos_to_pub;
nav_msgs::Odometry odom_to_pub;

bool have_pos = false;
bool have_vel = false;

void posCallback(const geometry_msgs::PoseStamped &msg)
{
  pos = msg;
  have_pos = true;
}

void velCallback(const geometry_msgs::TwistStamped &msg)
{
  vel = msg;
  have_vel = true;
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "trans");
  ros::NodeHandle nh;

  // get quaternion of q_w2v and  q_m2p from yaml
  Eigen::Quaterniond q_w2v, q_m2p;
  std::vector<double> q_w2v_vec(4), q_m2p_vec(4);

  const std::string &node_name = ros::this_node::getName();
  nh.getParam(node_name + "/" + "q_w2v", q_w2v_vec);
  nh.getParam(node_name + "/" + "q_m2p", q_m2p_vec);

  q_w2v = Eigen::Quaterniond(q_w2v_vec[0], q_w2v_vec[1], q_w2v_vec[2], q_w2v_vec[3]);
  q_m2p = Eigen::Quaterniond(q_m2p_vec[0], q_m2p_vec[1], q_m2p_vec[2], q_m2p_vec[3]);

  std::cout << "[TRANS] q_w2v: " << q_w2v.coeffs().transpose() << std::endl;
  std::cout << "[TRANS] q_m2p: " << q_m2p.coeffs().transpose() << std::endl;

  ros::Publisher pubPose = nh.advertise<geometry_msgs::PoseStamped>(
      "/mavros/vision_pose/pose", 10);
  ros::Publisher pubOdom = nh.advertise<nav_msgs::Odometry>(
      "/mavros/vision_pose/odom", 10);
  ros::Subscriber subPos = nh.subscribe("/pose", 10, posCallback);
  ros::Subscriber subVel = nh.subscribe("/vel", 10, velCallback);
  ros::Rate rate(50);

  while (ros::ok())
  {
    ros::spinOnce();

    pos_to_pub.header = pos.header;
    odom_to_pub.header = pos.header;

    pos_to_pub.pose.position = pos.pose.position;
    Eigen::Quaterniond q(pos.pose.orientation.w,
                         pos.pose.orientation.x,
                         pos.pose.orientation.y,
                         pos.pose.orientation.z);
    q = q_w2v * q * q_m2p;
    pos_to_pub.pose.orientation.w = q.w();
    pos_to_pub.pose.orientation.x = q.x();
    pos_to_pub.pose.orientation.y = q.y();
    pos_to_pub.pose.orientation.z = q.z();

    Eigen::Vector3d p(pos.pose.position.x,
                      pos.pose.position.y,
                      pos.pose.position.z);
    Eigen::Vector3d v(vel.twist.linear.x,
                      vel.twist.linear.y,
                      vel.twist.linear.z);

    p = q_w2v * p;
    v = q_w2v * v;

    pos_to_pub.pose.position.x = p(0);
    pos_to_pub.pose.position.y = p(1);
    pos_to_pub.pose.position.z = p(2);

    odom_to_pub.pose.pose = pos_to_pub.pose;

    odom_to_pub.twist.twist.linear.x = v(0);
    odom_to_pub.twist.twist.linear.y = v(1);
    odom_to_pub.twist.twist.linear.z = v(2);

    pubPose.publish(pos_to_pub);
    pubOdom.publish(odom_to_pub);
    rate.sleep();
  }

  return 0;
}
